/*********************************************************************
* File: NavigationSampleAgent.cpp
*
* Implements the navigation API sample. 
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
*********************************************************************/
#include "NavigationSampleAgent.h"
using namespace agents::samples;

#include "../../util/debug.h"
#include "../../util/util.h"
using namespace util;

#define TIMEOUT_NOT_DEFINED -1

NavigationSampleAgent::NavigationSampleAgent(edict_t *enity, int team, int model)
:Agent(enity, team, model)
{
	for(int i=0; i<Operations::max; i++)
	{
		records[i].timeout = TIMEOUT_NOT_DEFINED;
		records[i].enabled = false;
	}
	stop();
}

NavigationSampleAgent::~NavigationSampleAgent(){}

void NavigationSampleAgent::stop()
{
	for(int i=0; i<Operations::max; i++)
		records[i].enabled = false;
}

void NavigationSampleAgent::newRound()
{
	Agent::newRound();
	_isHolding = false;
}

void NavigationSampleAgent::think()
{
	IAgent& agent = *((IAgent*) this);

	if(_isHolding)
	{
		if(_holdTimeout > getWorld().getTime())
			getWaypointNavigation().hold();
		else
			_isHolding = false;
	}
	if(_isDucking)
	{
		if(_duckTimeout > getWorld().getTime())
			getNavigation().duck();
		else
			_isDucking = false;
	}

	for(int i=0; i<Operations::max; i++)
	{
		if(records[i].timeout != TIMEOUT_NOT_DEFINED)
			if(records[i].timeout <= getWorld().getTime())
			{
				records[i].timeout = TIMEOUT_NOT_DEFINED;
				records[i].enabled = false;
			}
	}

	if(records[Operations::moveForward].enabled)
		getNavigation().moveForward();
	if(records[Operations::moveBackwards].enabled)
		getNavigation().moveBackwards();
	if(records[Operations::moveLeft].enabled)
		getNavigation().moveLeft();
	if(records[Operations::moveRight].enabled)
		getNavigation().moveRight();
	if(records[Operations::turnLeft].enabled)
		getNavigation().rotateLeft();
	if(records[Operations::turnRight].enabled)
		getNavigation().rotateRight();

	ICommunication::MessageRecord record;
	if(getCommunication().receiveMsg(ICommunication::SayMessages::sayMsg, &record))
		handleMessage(record);
	else if(getCommunication().receiveMsg(ICommunication::SayMessages::sayTeamMsg, &record))
		handleMessage(record);
	else if(getCommunication().receiveMsg(ICommunication::SayMessages::sayDeadMsg, &record))
		handleMessage(record);
}
void NavigationSampleAgent::handleMessage(ICommunication::MessageRecord& record)
{
	char msgBuffer[1024];
	char command[32];
	char arg1[32];
	char arg2[32];

	strcpy(arg1, "");
	strcpy(arg2, "");
	sscanf(record.msg, "%s %s %s", command, arg1, arg2);

	if((strcmp(command,"position")==0) || (strcmp(command,"p")==0))
	{
		Vector position = getNavigation().getPosition();
		sprintf(msgBuffer, "My position is: %.2f %.2f %.2f\n", position.x, position.y, position.z);
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"angles")==0) || (strcmp(command,"a")==0))
	{
		Vector angles = getNavigation().getAngles();
		sprintf(msgBuffer, "My movement angles are: %.2f %.2f %.2f\n", angles.x, angles.y, angles.z);
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"moveF")==0) || (strcmp(command,"f")==0))
	{
		records[Operations::moveForward].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::moveForward].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::moveForward].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Moving forward...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"moveB")==0) || (strcmp(command,"b")==0))
	{
		records[Operations::moveBackwards].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::moveBackwards].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::moveBackwards].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Moving backwards...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"moveL")==0) || (strcmp(command,"l")==0))
	{
		records[Operations::moveLeft].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::moveLeft].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::moveLeft].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Moving left...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"moveR")==0) || (strcmp(command,"r")==0))
	{
		records[Operations::moveRight].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::moveRight].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::moveRight].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Moving right...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"turnL")==0) || (strcmp(command,"tL")==0))
	{
		records[Operations::turnLeft].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::turnLeft].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::turnLeft].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Rotating left...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"turnR")==0) || (strcmp(command,"tR")==0))
	{
		records[Operations::turnRight].enabled = true;
		if(util::isNullString(arg1))
			records[Operations::turnRight].timeout = TIMEOUT_NOT_DEFINED;
		else
			records[Operations::turnRight].timeout = getWorld().getTime() + atof(arg1);
		sprintf(msgBuffer, "Rotating right...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"jump")==0) || (strcmp(command,"j")==0))
	{
		getNavigation().jump();
		sprintf(msgBuffer, "Jumping...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"duck")==0) || (strcmp(command,"d")==0))
	{
		getNavigation().duck();
		_isDucking = true;
		_duckTimeout = getWorld().getTime() + 5.0;
		sprintf(msgBuffer, "Ducking for 5 seconds...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(record.msg,"moveToHere")==0) || (strcmp(command,"mH")==0))
	{
		stop();
		Vector senderOrigin = getNavigation().getPosition() + record.sender.radarUnit.direction * record.sender.radarUnit.distance;
		getFreeNavigation().moveToDestination(senderOrigin);
		sprintf(msgBuffer, "Moving towards your current position (%.2f %.2f %.2f)...\n", 
			senderOrigin.x, senderOrigin.y, senderOrigin.z);
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command, "moveToGoal")==0) || (strcmp(command,"mG")==0))
	{
		int destination = getWaypointNavigation().getGoalWaypointIndices()[
			RANDOM_LONG(0, getWaypointNavigation().getNumberOfGoalWaypoints())];
			getWaypointNavigation().moveToWaypoint(destination);
			sprintf(msgBuffer, "Moving to goal waypoint %d...\n", destination);
			getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command, "moveTo")==0)||(strcmp(command, "m")==0))
	{
		if(util::isNullString(arg1))
		{
			util::debug::printToServer("ERROR: Incorrect arguments. Syntax: moveTo <destinationWaypoint>");
			return;
		}

		int destination = atoi(arg1);
		getWaypointNavigation().moveToWaypoint(destination);
		sprintf(msgBuffer, "Moving to waypoint %d.\n", destination);
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command, "stop")==0) || (strcmp(command,"s")==0))
	{
		stop();
		getWaypointNavigation().stop();
		sprintf(msgBuffer, "Stopped.\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command, "hold")==0) || (strcmp(command,"h")==0))
	{
		getWaypointNavigation().hold();
		_isHolding = true;
		_holdTimeout = getWorld().getTime() + 5.0;
		sprintf(msgBuffer, "Holding for 5 seconds...\n");
		getCommunication().sayText(msgBuffer);
	}
	else if((strcmp(command,"isStuck")==0) || (strcmp(command,"sk")==0))
	{
		if(getCollisionHandler().isStuck())
			sprintf(msgBuffer, "I'm stuck. \n");
		else
			sprintf(msgBuffer, "I'm not stuck. \n");
		getCommunication().sayText(msgBuffer);
	}
	else if(strcmp(command,"dH")==0)
	{
		Vector senderOrigin = getNavigation().getPosition() + record.sender.radarUnit.direction * record.sender.radarUnit.distance;
		sprintf(msgBuffer, "Radar distance towards you is %.2f. \n", (getNavigation().getPosition() - senderOrigin).Length());
		getCommunication().sayText(msgBuffer);
	}
}